#include "quadpod.fp.h"

#if defined(FIXED_KINEMATICS)
#include "quadpod_data.h"
#else
const step_t c_step __attribute__ ((section(".progmem"))) = {
{{1098,2194,2268},{3356,2195,2109},{1997,2195,2109},{4255,2194,2268}}
};  
// Initial configuration for arms. 
step_t tmp_step;
#endif

// *** Constants
#if defined(FIXEDPOINT_MATH) //<<<

static const fix16_t __attribute__ ((progmem)) fix16_Xtable[] = {
0x00010000, 0x0000AAAB, 0x00005556, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00001C72, 0x000038E3, 0x00005556, 0x000071C7, 0x00008E39, 0x0000AAAB, 0x0000AAAB, 0x0000AAAB, 0x0000AAAB, 0x0000C71C, 0x0000E38E,
0x00005556, 0x000071C7, 0x00008E39, 0x0000AAAB, 0x0000AAAB, 0x0000AAAB, 0x0000AAAB, 0x0000C71C, 0x0000E38E, 0x00010000, 0x0000AAAB, 0x00005556, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00001C72, 0x000038E3, 
0x0000AAAB, 0x0000C71C, 0x0000E38E, 0x00010000, 0x00010000, 0x00010000, 0x00010000, 0x0000AAAB, 0x00005556, 0x00000000, 0x00001C72, 0x000038E3, 0x00005556, 0x00005556, 0x00005556, 0x00005556, 0x000071C7, 0x00008E39, 
0x00000000, 0x00001C72, 0x000038E3, 0x00005556, 0x00005556, 0x00005556, 0x00005556, 0x000071C7, 0x00008E39, 0x0000AAAB, 0x0000C71C, 0x0000E38E, 0x00010000, 0x00010000, 0x00010000, 0x00010000, 0x0000AAAB, 0x00005556 
};
/* 9/9    ,     6/9   ,    3/9    ,     0/9   ,    0/9    ,     0/9   ,     0/9   ,     1/9   ,     2/9   ,     3/9   ,     4/9   ,    5/9    ,     6/9   ,     6/9   ,     6/9   ,     6/9   ,    7/9    ,    8/9;
 * 3/9    ,     4/9   ,    5/9    ,     6/9   ,    6/9    ,     6/9   ,     6/9   ,     7/9   ,     8/9   ,     9/9   ,     6/9   ,    3/9    ,     0/9   ,     0/9   ,     0/9   ,     0/9   ,    1/9    ,    2/9;
 * 6/9    ,     7/9   ,    8/9    ,     9/9   ,    9/9    ,     9/9   ,     9/9   ,     6/9   ,     3/9   ,     0/9   ,     1/9   ,    2/9    ,     3/9   ,     3/9   ,     3/9   ,     3/9   ,    4/9    ,    5/9;
 * 0/9    ,     1/9   ,    2/9    ,     3/9   ,    3/9    ,     3/9   ,     3/9   ,     4/9   ,     5/9   ,     6/9   ,     7/9   ,    8/9    ,     9/9   ,     9/9   ,     9/9   ,     9/9   ,    6/9    ,    3/9;
 *
 * The XTable controlls the fixed gait 1-3-2-4. It reads the relative position of the arm on the movement vector. 
 * Each step consists of 12 position. Step starts in position 0 and goes position no.9. Position 10,11 are used to move arm back to position 0.
 * The table of x-positions must be aligned with the table for z-positions and y-position. While the alternation in z-position implements
 * the tilting movement, the y-position brings swaying - a lizard-like movement. Additionally z-table controls the operation of litfing
 * the  arm in position 10,11. 
*/

/*
static const fix16_t __attribute__ ((progmem)) fix16_Ytable_fwd[] = {
0x00000000, 0x00005555, 0x0000AAAA, 0x00010000, 0x00005555, 0xFFFFAAAC, 0xFFFEFFBF, 0xFFFF555A, 0xFFFFAAAC, 0x00000000, 0xFFFFAAAC, 0xFFFF555A, 0xFFFEFFBF, 0xFFFFAAAC, 0x00005555, 0x00010000, 0xFFFF555A, 0xFFFFAAAC 
}; */
/* 0/9    ,     3/9   ,     6/9   ,     9/9   ,     3/9   ,    -3/9   ,    -9/9   ,    -6/9   ,   -3/9    ,     0/9   ,    -3/9   ,   -6/9    ,   -9/9    ,   -3/9    ,    3/9    ,    9/9    ,   -6/9    ,   -3/9    
 * Ytable implements swaying, a lizard-like movement. The swaying brings higher value of the stability region. The CoG moved dynamicaly and
 * immitates the swaying movement of lizards body.
 */

//static const fix16_t __attribute__ ((progmem)) fix16_Ytable_rev[] = {
static const fix16_t __attribute__ ((progmem)) fix16_Ytable[] = {
0x00000000, 0xFFFFAAAC, 0xFFFF555A, 0xFFFEFFBF, 0xFFFFAAAC, 0x00005555, 0x00010000, 0x0000AAAA, 0x00005555, 0x00000000, 0x00005555, 0x0000AAAA, 0x00010000, 0x00005555, 0xFFFFAAAC, 0xFFFEFFBF, 0xFFFF555A, 0xFFFFAAAC 
};
/* 0/9    ,    -3/9   ,    -6/9   ,    -9/9   ,    -3/9   ,     3/9   ,     9/9   ,      6/9  ,     3/9   ,     0/9   ,     3/9   ,     6/9   ,     9/9   ,     3/9   ,    -3/9   ,    -9/9   ,    -6/9   ,    -3/9   
 * Ytable implements swaying, a lizard-like movement. The swaying brings higher value of the stability region. The CoG moved dynamicaly and
 * immitates the swaying movement of lizards body.
 */

static const fix16_t __attribute__ ((progmem)) fix16_thetas[] = {
// Straight movement. All arms in one direction
0x0000B505, 0xFFFF4AfA, 0x0000B505,0x0000B505,  // cos( 45),-sin( 45);sin( 45),cos( 45)
0x0000B505, 0x0000B505, 0xFFFF4AfA,0x0000B505,  // cos(315),-sin(315);sin(315),cos(315)
0xFFFF4AfA, 0x0000B505, 0xFFFF4AfA,0xFFFF4AfA,  // cos(225),-sin(225);sin(225),cos(225)
0xFFFF4AfA, 0xFFFF4AfA, 0x0000B505,0xFFFF4AfA,  // cos(135),-sin(135);sin(135),cos(135)
// This is for the rotational movement around the center of the quadpod
0x00000000, 0xFFFF0000, 0x00010000, 0x00000000, // cos(90),-sin(90),sin(90),cos(90) 
0x00000000, 0xFFFF0000, 0x00010000, 0x00000000, // cos(90),-sin(90),sin(90),cos(90) 
0x00000000, 0xFFFF0000, 0x00010000, 0x00000000, // cos(90),-sin(90),sin(90),cos(90) 
0x00000000, 0xFFFF0000, 0x00010000, 0x00000000, // cos(90),-sin(90),sin(90),cos(90) 
};

static const fix16_t __attribute__ ((progmem)) fix16_Ztable_fwd[] = {
0x00000000, 0x00150000, 0x00150000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000,
0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00150000, 0x00150000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000,
0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00150000, 0x00150000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000,
0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00150000, 0x00150000
};
static const fix16_t __attribute__ ((progmem)) fix16_Ztable_rev[] = {
0x00000000, 0x00150000, 0x00150000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000,
0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00150000, 0x00150000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000,
0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00150000, 0x00150000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000,
0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x00150000, 0x00150000
};
/* Alternative Ztable that helps to tilt the quadpod before it starts shifting the arms.
 * Two tables are required, because the tilting for forward movement is different than
 * for the reverse. The kick helps a bit, but it should be probably even larger. Make
 * it more in the next version (twice?)
 */

static const fix16_t __attribute__ ((progmem)) fix16_directions[] = {
/*  0 */ 0x00000000, 0x00010000, 0xFFFF0000, 0x00000000, // cos(270),-sin(270),sin(270),cos(270) 
/*  1 */ 0x00004241, 0x0000F746, 0xFFFF08BA, 0x00004241, // cos(285),-sin(285),sin(285),cos(285) 
/*  2 */ 0x00008000, 0x0000DDB3, 0xFFFF224D, 0x00008000, // cos(300),-sin(300),sin(300),cos(300) 
/*  3 */ 0x0000B827, 0x0000B1D5, 0xFFFF4E2A, 0x0000B827, // cos(316),-sin(316),sin(316),cos(316) 
/*  4 */ 0x0000DDB3, 0x00008000, 0xFFFF8000, 0x0000DDB3, // cos(330),-sin(330),sin(330),cos(330) 
/*  5 */ 0x0000F746, 0x00004241, 0xFFFFBDBF, 0x0000F746, // cos(345),-sin(345),sin(345),cos(345) 
/*  6 */ 0x00010000, 0x00000000, 0x00000000, 0x00010000, // cos(0),-sin(0),sin(0),cos(0) 
/*  7 */ 0x0000F746, 0xFFFFBDBF, 0x00004241, 0x0000F746, // cos(15),-sin(15),sin(15),cos(15) 
/*  8 */ 0x0000DDB3, 0xFFFF8001, 0x00007FFF, 0x0000DDB3, // cos(30),-sin(30),sin(30),cos(30) 
/*  9 */ 0x0000B1D5, 0xFFFF47D8, 0x0000B827, 0x0000B1D5, // cos(46),-sin(46),sin(46),cos(46) 
/* 10 */ 0x00008000, 0xFFFF224D, 0x0000DDB3, 0x00008000, // cos(60),-sin(60),sin(60),cos(60) 
/* 11 */ 0x00004241, 0xFFFF08BA, 0x0000F746, 0x00004241, // cos(75),-sin(75),sin(75),cos(75) 
/* 12 */ 0x00000000, 0xFFFF0000, 0x00010000, 0x00000000, // cos(90),-sin(90),sin(90),cos(90) 
};

/*  3  0x0000B504, 0x0000B504, 0xFFFF4AFC, 0x0000B504, // cos(315),-sin(315),sin(315),cos(315) */ 
/*  9  0x0000B504, 0xFFFF4AFC, 0x0000B504, 0x0000B504, // cos(45),-sin(45),sin(45),cos(45) */ 
/* Replaced two lines in the table above. The reason is that the rotation of the translated point (+36,+36)
 * produces division by zero. It can be quickly overcome by using the angles 46, 316 respectively
 */


static const f16_gait_type_t __attribute__ ((progmem)) fix16_gait[] = {
/* CRAB */ {{{0xFFCCFFFF,0x00000000},{0x00330000,0x00000000},{0x00330000,0x00000000},{0xFFCCFFFF,0x00000000}}},
/* FROG */ {{{0xFFDBFFFF,0x00240000},{0x00240000,0x00240000},{0x00240000,0xFFDBFFFF},{0xFFDBFFFF,0xFFDBFFFF}}}
};
/* This table controls the gait style. The Gait actually remain the same as defined in XTable, this table 
 * controls the displacement vector applied to each arm. Direction vector is calculated for shoulder pivot
 * and the displaycement vector positiones the tip of the arm. It can be a crab-like movement, where the 
 * tip od arm is on the same y coordinate as the pivot of shoulder or can be displaced along the diagonal axis
 * connecting shoulder points of opposed arms and running though the CoG.
 *
 *  CRAB:
 *  -----
 *
 *  +  o   o  +
 *
 *  +  o   o  +
 *
 *
 * FROG:
 * -----
 *
 *  +         +
 *     o   o
 *
 *     o   o
 *  +         +
 *
 * where + - tip of arm
 *       o - shoulder joint (pivot)
 */

#endif
// >>>

// *** Variable Declarations
// <<<
unsigned char sreg;
uint8_t	oop;
uint8_t step_last_known;

speed_t speed;

volatile ocr1a_t servo_cntrls;
volatile servo_cntrl_t servo;
volatile move_t move;
volatile gait_cntrl_t gait_cntrls;
volatile decoder_t ird;

#if !defined(FIXEDPOINT_MATH) && defined(FLOATINGPOINT_MATH)

double a1 = 0.3071;
double temp;

vector_t v = {-58.69,13.435,51.0}; // Test vector for math library

#endif

#if defined(FIXEDPOINT_MATH) && !defined(FLOATINGPOINT_MATH)

fix16_t a1_16;
fix16_t res_16;
fix16_t height_16 = 0x00330000; // set it to 51 as default height

f16_vector_t f16_v = {0xFFC54F5B, 0xD6F5C, 0x330000}; // Test vector for fixpoint library {-58.69,13.435,51.0}
f16_vector_t f16_data = {0x00000000, 0x00000000, 0x00000000}; // Test vector for fixpoint library
f16_vector_t f16_v_res; // Test vector for fixpoint library

                     //{ cos()    ,   -sin()  ,   sin()   ,    cos()  }
//fix16_t r_dir[4]   = {0x0000DDB4, 0xFFFF7FFF, 0x00008000, 0x0000DDB4}; // rotate 30 deg.
fix16_t r_dir[4]   = {0x0000B505, 0x0000B505, 0xFFFF4AFA, 0x0000B505}; // rotate 315 deg.

fix16_t r_servo[4] = {0x00000000, 0x00000000, 0x00000000, 0x00000000}; // rotate 00 deg.

fix16_t angle = 0x0000860B; // 30 deg = 0.5236 rad.
fix16_t step = 0x00400000; // Step = 64 mm 
angles_t a;
fix16_t	x;

uint8_t idx;
uint8_t	arm = 0;
uint8_t	k = 0;
#endif
//>>>

#ifdef USART_DEBUG //<<<
uint8_t i;
uint8_t buffer[32];
uint8_t servo_trimming_index = 10;
uint8_t usart_manual_cntrl = 0;

#endif // USART_DEBUG
//>>>

uint8_t buffer[32];
volatile uint8_t usart_data;

#if defined( __AVR_ATmega8__ )
ISR(USART_RXC_vect)
/* USART Interrupt handler
 * -----------------------
 * This routine handles the interrupt flag "USART RX" signal.
 * Fetches the character, check the framing error
 * and evaluates the received byte. Either sends it 
 * to LCD for display or performs an LCD instruction
 */
{ // <<<
	uint8_t c;

	//Store data to temp
	c = UDR;   
	// Check the framing error, defect frames are skipped and must be re-transmitted.
	if (bit_is_clear(UCSRA, FE)) {
		usart_data = c;
	} else {
		USART_send_str_P ( (uint8_t*) PSTR("Transmission Error\r\n"));
	}
}    // >>>
#elif defined( __AVR_ATmega168__ )
ISR(USART_RX_vect)
{ // <<<
	uint8_t c;

	//Store data to temp
	c = UDR0;   
	// Check the framing error, defect frames are skipped and must be re-transmitted.
	if (bit_is_clear(UCSR0A, FE0)) {
		usart_data = c;
	} else {
		USART_send_str_P ( (uint8_t*) PSTR("Transmission Error\r\n"));
	}
} // >>>
#else
#error "Unsupported AVR device. This code compiles for ATmega8, ATmega168 or ATmega328"
#endif


ISR(TIMER1_COMPA_vect)
/* Interrupt handler for compare match.  The controller fires this interrupt
 * every time the internal counter reaches the pre-set value (OCR1A). The
 * handler than sends the control signal to a selected servo by strobing the
 * clock signal on the servo shield bank. The time delay between tha last
 * strobe and the current one defines the length of the control pulse for a
 * servo. The handler manages bank switching in servo shield. Every bank
 * connects 6 servos only, the robot requires 12 servos to move.
 */
{ //<<<
	servo_cntrls.cntr += 1;
	servo_cntrls.cntr %= BANK_MAX_SIZE;

	if( 0 == servo_cntrls.cntr ) {
		servo_cntrls.bank ^= SIGNALED;
		if( servo_cntrls.bank ) {
			SERVOSHIELD_CTRL |= _BV(MR_BANK_1);
			SERVOSHIELD_CTRL &= ~_BV(MR_BANK_0);
		} else {
			SERVOSHIELD_CTRL |= _BV(MR_BANK_0);
			SERVOSHIELD_CTRL &= ~_BV(MR_BANK_1);
		}
	} 
	/* First task: swap the servo shield bank every 6 servos. 
	 * Each johnson counter control 6 servos only. 
	 */

	SERVOSHIELD_CTRL |= _BV(CP0);
	_delay_us(2);
#ifdef _TEST_MODE
	OCR1A = (uint16_t) NEUTRAL;
#else
	OCR1A = (uint16_t) (servo.current[servo_cntrls.servo_index] > 0 ? servo.current[servo_cntrls.servo_index] : NEUTRAL);
#endif
	servo_cntrls.servo_index += 1;
	servo_cntrls.servo_index %= COUNT_OF_SERVOS;
	if( 0 == servo_cntrls.servo_index ) {
		servo_cntrls.dirty ^= SIGNALED;
	}
	SERVOSHIELD_CTRL &= ~_BV(CP0);
	/* Second task: strobe the CP0 pin of 4017 for minimum 2us. 
	 * load the control byte from latch buffer into register OCR1A.
	 * Invalidate the latch buffer content after emptying latch buffer.
	 * by setting the "dirty" flag in servo_cntrls structure.
	 */

} //>>>

ISR(TIMER0_COMPA_vect)
/* Timer0 increases its count once in 1,5 miliseconds. 
 * The counting is reset each by the falling edge 
 */ 
{ // <<<
	ird.cntr++;	
	if( TOP == ird.cntr )
		ird.debounce++;
} // >>>

ISR(INT0_vect)
/* The external interrupt is connected to the output PIN of the IR decoder - SO1838
 * (equivalent to TSOP31238). The decoder receives the IR signal, removes the modulation
 * and generates inverted signal on its output pin. uC catches the falling edge on the 
 * INT0, whihc corresponds with rising edge of the signal. The signal is preprocessed
 * in a state automat in the interrupt and the resulting value is used in the application 
 * logic. The digram of the FSM is in the header file for ir decoder.
 */
{ // <<<
   static uint8_t __run = 0;
	uint8_t __irc;
	
	__irc = ird.cntr;
	ird.cntr = 0;
	TCNT0 = (uint8_t) 0;

	if( FINISHED != ird.status ) { 
	
		if( DECODING == ird.status ) {
			if ( __run > 7 ) {
				ird.status = FINISHED;
			} else {
	       	if( __irc < 2 ) {
					if( __run == 0 )
						ird.code = __irc;
					else
						ird.code |= (__irc<<__run);
					__run++;
				} else {
					ird.status = IDLE;
				}	
			}
		} else {
	
			if( STEADY == ird.status ) {
				if( __irc == 3 ) { 
					ird.status = DECODING;
					__run = 0;
				} else {
					if( __run > 7 )
						ird.status = IDLE;
					__run++;
				}
			} else {
	
				if( __irc == 8 ) {
					ird.status = STEADY;
					__run = 0;
				}
			}
		}
	}
} // >>>

int main() // ******************** M A I N ****************************
{ //<<<
	usart_data = 0x0;

	init();
	fill_servo_page_P( (move_t) move );  							// Init the future structure
	fill_servo_page_P( (move_t) move );  							// Init the past structure
	fill_servo_buffer( (move_t) move, (speed_t) speed );  // Init the current structure

	/* <<<
			sprintf_P((char*) buffer, PSTR("INIT"));
			temp =  atan(a1);
			sprintf_P((char*) buffer, PSTR("INIT %f"), temp);
			temp =  acos(a1);
			sprintf_P((char*) buffer, PSTR("INIT %f"), temp);
			temp =  sqrt(a1);
			sprintf_P((char*) buffer, PSTR("INIT %f"), temp);
			a1_16 = (fix16_t) fix16_from_dbl( (double) a1 );
			temp = (double) fix16_to_dbl(a1_16);
			res_16 = (fix16_t) fix16_atan((fix16_t) a1_16 );
			temp = fix16_to_dbl(res_16);
			res_16 = (fix16_t) fix16_acos((fix16_t) a1_16 );
			temp = fix16_to_dbl(res_16);
			res_16 = (fix16_t) fix16_atan((fix16_t) a1_16 );
			temp = fix16_to_dbl(res_16);
			temp =  acos(a1);
			sprintf_P((char*) buffer, PSTR("INIT %f"), temp);
			temp =  atan(a1);
			sprintf_P((char*) buffer, PSTR("INIT %f"), temp);

			a1_16 = fix16_from_dbl( (double) 0.7071 );
			fix16_t a2_16 = fix16_from_dbl( (double) 3.2979 );

			fix16_t res_16 = fix16_add( (fix16_t) a1_16, (fix16_t) a2_16 ); //4.005
			temp =  fix16_to_dbl((fix16_t) res_16 );
			res_16 = fix16_sub( (fix16_t) a2_16, (fix16_t) a1_16 ); // 2.5908
			temp = fix16_to_dbl((fix16_t) res_16 );
			res_16 = fix16_mul( (fix16_t) a1_16, (fix16_t) a2_16 ); // 2.33194509
			temp = fix16_to_dbl((fix16_t) res_16 );
			res_16 = (fix16_t) fix16_sqrt((fix16_t) a1_16 );
			temp = fix16_to_dbl((fix16_t) res_16 );

			sprintf_P((char*) buffer, PSTR("INIT"));
			res_16 = fix16_sub( (fix16_t) 0x00010000, (fix16_t) 0x00020000 ); // -1
			temp = fix16_to_dbl(res_16);
			sprintf_P((char*) buffer, PSTR("%f"), temp);

			// >>>
	*/

	servo_shield_port_init();		// Initialize HW for servo shield
	servo_shield_init();				// Initialize servo shield
	servo_shield_cntrl( ENABLE );	// enable servo shield control circuit

	ir_sensor_init();
	ir_sensor_cntrl( ENABLE );
	irq0_init();
	irq0_cntrl( ENABLE );
	/* Initialize the IR Decoder. Decoder use External interrupt min INT0 and
	 * the Timer0. The timer is couting the 1,5 miliseconds intervals between
	 * the signals incoming on the INT0 from the external IR-decoder. 
	 */

	wdt_enable(WDTO_2S); // restart the processor if something hangs for longer than 1 second.

#ifdef USART_DEBUG
//<<<
	USART_init();
	_delay_ms(200);
	USART_send_str_P( (uint8_t*) PSTR("START\r\n" ));
#endif
//>>>

	sei();

	while(1) {
		switch( usart_data ) {
		// <<<
#ifdef USART_DEBUG //<<<
			case 'i':
#ifndef USART_DEBUG_SMALL_SIZE
				sprintf_P( (char*) buffer, PSTR("IR=%d\r\n"), ird.code );
				USART_send_str( (uint8_t*) buffer );
				sprintf_P( (char*) buffer, PSTR("step=%d\r\n"), move.stepper_current );
				USART_send_str( (uint8_t*) buffer );
#else				
				USART_send_str_P( (uint8_t*) PSTR("IR code="));
				itoa( (int) ird.code, (char*) buffer, 10 );
				USART_send_str( (uint8_t*) buffer );

				USART_send_str_P( (uint8_t*) PSTR("\r\nstep="));
				itoa( (int) move.stepper_current, (char*) buffer, 10 );
				USART_send_str( (uint8_t*) buffer );

				USART_send_str_P( (uint8_t*) PSTR("\r\ngait="));
				itoa( (int) move.gait, (char*) buffer, 10 );
				USART_send_str( (uint8_t*) buffer );

				USART_send_str_P( (uint8_t*) PSTR("\r\nAngle ="));
				itoa( (int) gait_cntrls.angle_index, (char*) buffer, 10 );
				USART_send_str( (uint8_t*) buffer );
				USART_send_str_P( (uint8_t*) PSTR("\r\n"));
#endif // USART_DEBUG_SMALL_SIZE
				break;

			case 'I':
				for( i=0; i<COUNT_OF_SERVOS;i++ ) {
#ifndef USART_DEBUG_SMALL_SIZE
					sprintf_P( (char*) buffer, PSTR("Servo=%d,ORC1A=%d\r\n"), i, servo.current[i] );
					USART_send_str( (uint8_t*) buffer );
#else
					USART_send_str_P( (uint8_t*) PSTR("Servo="));
					itoa( (int) i, (char*) buffer, 10 );
					USART_send_str( (uint8_t*) buffer );
					USART_send_str_P( (uint8_t*) PSTR(",OCR1A="));
					itoa( (int) servo.current[i], (char*) buffer, 10 );
					USART_send_str( (uint8_t*) buffer );
					USART_send_str_P( (uint8_t*) PSTR("\r\n"));
#endif // USART_DEBUG_SMALL_SIZE
				}
            for( i=0; i<COUNT_OF_ARMS; i++ ) {
					USART_send_str_P( (uint8_t*) PSTR("temp (G,A,B) = "));
					itoa( (int) tmp_step.arm[i].servo_shoulder, (char*) buffer, 10 );
					USART_send_str( (uint8_t*) buffer );
					USART_send_str_P( (uint8_t*) PSTR(", "));
					itoa( (int) tmp_step.arm[i].servo_elbow, (char*) buffer, 10 );
					USART_send_str( (uint8_t*) buffer );
					USART_send_str_P( (uint8_t*) PSTR(", "));
					itoa( (int) tmp_step.arm[i].servo_anckle, (char*) buffer, 10 );
					USART_send_str( (uint8_t*) buffer );
					USART_send_str_P( (uint8_t*) PSTR("\r\n"));
				}
				break;

			case 'N':
				servo_trimming_index += 1;
				servo_trimming_index %= COUNT_OF_SERVOS;
				USART_send_str_P( (uint8_t*) PSTR("trimmed servo="));
				itoa( (int) servo_trimming_index, (char*) buffer, 10 );
				USART_send_str( (uint8_t*) buffer );
				USART_send_str_P( (uint8_t*) PSTR("\r\n"));
				break;

			case '!':
				for( i=0; i<COUNT_OF_SERVOS;i++ ) {
					servo.current[i] = NEUTRAL;
				}
				break;

			case '+':
				servo.current[servo_trimming_index] += 6;
				break;

			case '-':
				servo.current[servo_trimming_index] -= 6;
				break;

			case 'm':
				usart_manual_cntrl ^= SIGNALED;
				break;

#endif // >>> USART_DEBUG

			case 'V':
				speed.max = LINREG_NO_OF_STEPS;
				break;

			case 'v':
				speed.max += 2;
				break;

			case 'f':
				if( move.cntrl == STOP ) {
					move.cntrl = FORWARD;
				} else {
					move.cntrl_queue = FORWARD;
					move.cntrl_request = movement_change_start;
				}
 				gait_cntrls.kinematics_dirty = SIGNALED;
				break;

			case 'r':
				if( move.cntrl == STOP ) {
					move.cntrl = REVERSE;
				} else {
					move.cntrl_queue = REVERSE;
					move.cntrl_request = movement_change_start;
				}
 				gait_cntrls.kinematics_dirty = SIGNALED;
				break;

			case 's':
				move.cntrl = STOP;
//				gait_cntrls.kinematics_dirty = SIGNALED;
				break;

/*
			case 'a':
				if( gait_cntrls.angle_index < MAX_ANGLE_INDEX ) {
 					gait_cntrls.angle_index += 1;
				} else {
 					gait_cntrls.angle_index = 1;
					if( move.cntrl == REVERSE )
						move.cntrl = FORWARD;
					else
						move.cntrl = REVERSE;
				}
				gait_cntrls.angle_dirty = SIGNALED;
				break;

			case 'A':
				if( gait_cntrls.angle_index > MIN_ANGLE_INDEX ) {
 					gait_cntrls.angle_index -= 1;
				} else {
 					gait_cntrls.angle_index = 11;
					if( move.cntrl == REVERSE )
						move.cntrl = FORWARD;
					else
						move.cntrl = REVERSE;
				}
				gait_cntrls.angle_dirty = SIGNALED;
				break;
*/

			case 'H':
				height_16 += 0x000A0000; // Add 10mm to height
 				gait_cntrls.kinematics_dirty = SIGNALED;
				break;

			case 'h':
				height_16 -= 0x000A0000; // Deduce 10mm from the height
 				gait_cntrls.kinematics_dirty = SIGNALED;
				break;

			default:
				break;
		}

		usart_data = 0x0;
		//>>>

		if( FINISHED == ird.status ) {
		// <<<
#ifdef USART_DEBUG //<<<
#ifdef USART_DEBUG_SMALL_SIZE
			USART_send_str_P( (uint8_t*) PSTR("\r\nIR Finished \r\ncode="));
			itoa( (int) ird.code, (char*) buffer, 10 );
			USART_send_str( (uint8_t*) buffer );

			USART_send_str_P( (uint8_t*) PSTR("\r\ndebounce="));
			itoa( (int) ird.debounce, (char*) buffer, 10 );
			USART_send_str( (uint8_t*) buffer );
#endif // USART_DEBUG_SMALL_SIZE
#endif // USART_DEBUG >>>

			if( ird.debounce > 3 ) {
				switch( ird.code ) {
					case IRC_FORWARD:
#ifdef USART_DEBUG //<<<
						servo.current[servo_trimming_index] += 6;
#else // >>>
						//					move.cntrl = FORWARD;
						//					gait_cntrls.kinematics_dirty = SIGNALED;
						usart_data='f';
						// <<< #ifdef USART_DEBUG
#endif // >>> 
						break;

					case IRC_REVERSE:
#ifdef USART_DEBUG //<<<
						servo.current[servo_trimming_index] -= 6;
#else // >>>
						//					move.cntrl = REVERSE;
						//					gait_cntrls.kinematics_dirty = SIGNALED;
						usart_data='r';
						// <<< #ifdef USART_DEBUG
#endif // >>>
						break;

					case IRC_SLOWER:
						usart_data='v';
						break;

					case IRC_FASTER:
						usart_data='V';
						break;

					case IRC_HIGHER:
						usart_data='H';
						break;

					case IRC_LOWER:
						usart_data='h';
						break;

					case IRC_RIGHT:
						usart_data='a';
						break;

					case IRC_LEFT:
						usart_data='A';
						break;

					case IRC_STRAIGHT:
						move.movement=STRAIGHT;
						move.cntrl_request = movement_change_start;
						break;

					case IRC_ROTATE:
						move.movement=ROTATE;
						move.cntrl_request = movement_change_start;
						break;

					case IRC_STOP:
						move.cntrl = STOP;
						break;

					case IRC_GAIT:
						move.gait ^= SIGNALED;
						// Swap the gait between CRAB and TECHNICAL
						break;
				}
				ird.debounce = 0;
				while( ird.code > 0 )
					ird.code = 0;
				ird.status = IDLE;
#ifdef USART_DEBUG //<<<
#ifdef USART_DEBUG_SMALL_SIZE
				USART_send_str_P( (uint8_t*) PSTR("\r\nIR reset \r\ncode="));
				itoa( (int) ird.code, (char*) buffer, 10 );
				USART_send_str( (uint8_t*) buffer );

				USART_send_str_P( (uint8_t*) PSTR("\r\ndebounce="));
				itoa( (int) ird.debounce, (char*) buffer, 10 );
				USART_send_str( (uint8_t*) buffer );

				USART_send_str_P( (uint8_t*) PSTR("\r\nstatus="));
				itoa( (int) ird.status, (char*) buffer, 10 );
				USART_send_str( (uint8_t*) buffer );
#endif // USART_DEBUG_SMALL_SIZE
#endif // USART_DEBUG >>>
			}
		}
		// >>>

		if( SIGNALED == gait_cntrls.angle_dirty ) {
			//<<<
			gait_cntrls.angle_dirty ^= SIGNALED; // free the angle request ...
			r_dir[0] = (int32_t) pgm_read_dword( &fix16_directions[gait_cntrls.angle_index*4] );
			r_dir[1] = (int32_t) pgm_read_dword( &fix16_directions[(gait_cntrls.angle_index*4)+1] );
			r_dir[2] = (int32_t) pgm_read_dword( &fix16_directions[(gait_cntrls.angle_index*4)+2] );
			r_dir[3] = (int32_t) pgm_read_dword( &fix16_directions[(gait_cntrls.angle_index*4)+3] );
 			gait_cntrls.kinematics_dirty = SIGNALED; // set the kinematics request
		}
		/* Refresh the rotation matrix, if angle got chnaged:
		 *    cos(135),-sin(135);sin(135),cos(135)
		 *
		 *    r_dir[0] = fix16_cos( direction_angle );
		 *    r_dir[3] = r_dir[0];
		 *    r_dir[2] = fix16_sin( direction_angle );
		 *    r_dir[1] = fix16_mul( fix16_minus_one, r_dir[2] );
		 */
		//>>>

		if( SIGNALED == gait_cntrls.kinematics_dirty ) {
      //<<<
			gait_cntrls.kinematics_dirty ^= SIGNALED;
			/* tento blok se vola jenom v pripade, ze se spotreboval
			 * uplne cely bufer linearni regrese mezi past/future pozici.
			 * teprve potom je mozne urcit novou future pocici.
			 */
  			if( move.cntrl == REVERSE ) {
  				if( move.stepper_current == 0 ) {
  					move.stepper_current = STEP_TAB_SIZE;
  				} 
  			}
  			move.stepper_current += move.cntrl; 
  			move.stepper_current %= STEP_TAB_SIZE;

			switch( move.cntrl_request ) {
				// <<<
				case movement_change_finish:
					move.cntrl = move.cntrl_queue;
					move.cntrl_request = movement_no_change;

				case movement_change_start:
					if( move.stepper_current == 0 ) {
						move.cntrl_request = movement_change_finish;
					}
					if( move.stepper_current == 9 ) {
						move.cntrl_request = movement_change_finish;
					}

				case movement_no_change:
				default:
					for( arm = 0; arm < 4; arm++ ) {
						determine_next_step( arm, move.stepper_current );
					}
			} // >>>
		}
		//>>>

		if( SIGNALED == servo_cntrls.dirty ) {
			//<<<
			servo_cntrls.dirty ^= SIGNALED;

			if( speed.index < speed.max ) {
				speed.index += 1;
				fill_servo_buffer( (move_t) move, (speed_t) speed );
			} else {
				/* spotrebovan cely regresni interval mezi minulou a budouci
				 * pozici serv, nakopiruj novou budouci pozici, pokud robot
				 * nestoji.
				 */
				if( move.cntrl != STOP ) {
				  	fill_servo_page( (move_t) move );
				  	speed.index = 0;
				  	gait_cntrls.kinematics_dirty = SIGNALED;
				}
				/* Vyvolej prepocet nove pozice po ulozeni 
				 * aktualni do budouciho buferu serv a urceni
				 * smeru noveho kroku.
				if( move.cntrl == REVERSE ) {
					if( move.stepper_current == 0 ) {
						move.stepper_current = STEP_TAB_SIZE;
					} 
				}
				move.stepper_current += move.cntrl; 
				move.stepper_current %= STEP_TAB_SIZE;
				 */
//  				gait_cntrls.kinematics_dirty = SIGNALED;
#ifdef USART_DEBUG // <<<
  				if( 1 == usart_manual_cntrl ) {
  					move.cntrl = STOP;
  				}
				/* Zastav pohyb robota po spotrebovani vsech regresnich kroku
				 * mezi budoucim a minulym stavem drahy serv ...
				 */
#endif //>>>
			} 
		} //>>>

		wdt_reset();
		sleep_mode(); // may not be required, as the robot will repeatedly search for direction?
	}

	return 0;
} //>>>


void init( void ) // ********* F U N C T I O N S ********* 
{ //<<<
	servo_cntrls.cntr = 0;
	servo_cntrls.bank = 1;
	servo_cntrls.dirty = 0;
	servo_cntrls.servo_index = 0;
	step_last_known = INF; // INFINITY

	move.movement = STRAIGHT;
	move.stepper_current = 0;
	move.stepper_previous = INF;
	move.cntrl_request = movement_no_change;
#ifdef USART_DEBUG
	move.cntrl = STOP;
#else
//	move.cntrl = FORWARD;
	move.cntrl = STOP;
#endif
	move.gait=FROG;

	speed.max = LINREG_NO_OF_STEPS;
	speed.index = speed.max;

	gait_cntrls.kinematics_dirty = 0;
	gait_cntrls.angle_dirty = SIGNALED;
	gait_cntrls.angle_index = INIT_ANGLE_INDEX;

	ird.cntr = 0;
	ird.status = IDLE;
} //>>>


void irq0_init( void )
{ // <<<
#if defined( __AVR_ATmega8__ )
   MCUCR &= ~_BV(ISC00); 
   MCUCR |= _BV(ISC01); 
#elif defined(__AVR_ATmega168__ )
	EICRA &= ~_BV( ISC00 ); 
	EICRA |= _BV( ISC01 ); 
#else
#error "Unsupported AVR device. This code compiles for ATmega8, ATmega168 or ATmega328"
#endif
/* Falling edge of INT0 triggers IRQ.
 * Read EFIR register, bit 0 is the IRQ flag, instead of generating extra interrupt. Read the EFIR in the 
 * timer/counter user to time the servo shield. 
 * When an edge or logic change on the INT0 pin triggers an interrupt request, INTF0 becomes set
 * (one). If the I-bit in SREG and the INT0 bit in EIMSK are set (one), the MCU will jump to the cor-
 * responding Interrupt Vector. The flag is cleared when the interrupt routine is executed.
 * Alternatively, the flag can be cleared by writing a logical one to it. This flag is always cleared
 * when INT0 is configured as a level interrupt.
 */ 
} // >>>


void irq0_cntrl( type_t mode )
{ // <<<
	switch( mode )
	{
		case ENABLE:
#if defined( __AVR_ATmega8__ )
   	GICR |= _BV(INT0);
#elif defined(__AVR_ATmega168__ )
		EIMSK |= _BV(INT0);
#else
#error "Unsupported AVR device. This code compiles for ATmega8, ATmega168 or ATmega328"
#endif
		break;
		case DISABLE:
#if defined( __AVR_ATmega8__ )
   	GICR &= ~_BV(INT0);
#elif defined(__AVR_ATmega168__ )
		EIMSK &= ~_BV(INT0);
#else
#error "Unsupported AVR device. This code compiles for ATmega8, ATmega168 or ATmega328"
#endif
		break;
	}
} // >>>


void ir_sensor_init() 
{ // <<<
	TCCR0A &= ~_BV(COM0A0);
	TCCR0A &= ~_BV(COM0A1);
	// Configure OC0A for normal operation

	TCCR0A &= ~_BV(WGM00);
	TCCR0A |= _BV(WGM01);
	TCCR0B &= ~_BV(WGM02);
	// Configure CTC mode on Timer0

	TCCR0B |= _BV(CS02); // Set Prescale = 256

	TCNT0 = (uint8_t) 0;
	OCR0A = (uint8_t) IRDCNTR; 
	/* Reset the counter and preset the top value to 86 (IR Decoder Counter)
	 */
} // >>>


void ir_sensor_cntrl( type_t mode )
{ // <<<
	switch( mode )
	{
		case DISABLE:
#if defined(__AVR_ATmega168__)
			TIMSK0 &= ~_BV(OCIE0A);
#else
#error "IR connectivity is supported on Atmega168 only!"
#endif
		break;
		case ENABLE:
#if defined(__AVR_ATmega168__)
			TIMSK0 |= _BV(OCIE0A);
#else
#error "IR connectivity is supported on Atmega168 only!"
#endif
		break;
	}
} // >>>


void determine_next_step( uint8_t arm, uint8_t stepper_current  )
{ // <<<
	// x = (step/3) * Ytable(k);
	// y = (step/2) - (Xtable[arm,k]*step
	f16_data.x = f16_sway( (fix16_t) step, (uint8_t) stepper_current );
	f16_data.y = f16_step( (fix16_t) step, (uint8_t) arm, (uint8_t) stepper_current );
	// f16_data.x = 0x00000000;

	// Rotate the direction ...
	f16_v_res = f16_rotate_m( &f16_data, r_dir );

	// TODO - musi se pridat rezim pro podporu rotacniho pohybu, (-51,0) pro vsechny nohy.
	if( move.movement == STRAIGHT ) {
		f16_v_res.x = fix16_add( f16_v_res.x, (uint32_t) pgm_read_dword( &fix16_gait[move.gait].arm[arm].x )); 
		f16_v_res.y = fix16_add( f16_v_res.y, (uint32_t) pgm_read_dword( &fix16_gait[move.gait].arm[arm].y )); 
	} else {
		f16_v_res.x = fix16_add( f16_v_res.x, 0xFFCCFFFF ); 
	}
	/* Robot's arm can move following one vector or rotate on one spot.
	 * The type of movement is determined by adding the displacement
	 * constant to the x before rorating the vector. Displaycement vector
	 * varies for different gait type and movement type.    
	 */

	r_servo[0] = (int32_t) pgm_read_dword( &fix16_thetas[(arm*4)+move.movement] );
	r_servo[1] = (int32_t) pgm_read_dword( &fix16_thetas[((arm*4)+1)+move.movement] );
	r_servo[2] = (int32_t) pgm_read_dword( &fix16_thetas[((arm*4)+2)+move.movement] );
	r_servo[3] = (int32_t) pgm_read_dword( &fix16_thetas[((arm*4)+3)+move.movement] );
	f16_data = f16_rotate_m( &f16_v_res, r_servo );
	// Get the rotation angle for each arm and rotate the vector

	idx = arm * XTABLE_X_SIZE + stepper_current;

	if( move.cntrl == REVERSE ) {
		f16_data.z = (int32_t) pgm_read_dword( &fix16_Ztable_rev[idx] );
	} else {
		f16_data.z = (int32_t) pgm_read_dword( &fix16_Ztable_fwd[idx] );
	}

	if( f16_data.z != 0x00000000 ) {
		//					f16_data.z = fix16_add( f16_data.z, 0x00330000 ); // +51 (defautt height)
		f16_data.z = fix16_add( f16_data.z, height_16 ); // +51 (defautt height)
	} else {
		//					f16_data.z = 0x00330000; // set it to 51 as default height
		f16_data.z = height_16; // set it to 51 as default height
	}
	a = (angles_t) f16_p2a( (f16_vector_t *) &f16_data );
	//					sprintf_P((char*) buffer, PSTR("INIT"));
	tmp_step.arm[arm].servo_shoulder = a.gamma;
	tmp_step.arm[arm].servo_elbow = a.alpha;
	tmp_step.arm[arm].servo_anckle = a.beta;
}
// >>>


void servo_shield_port_init()
{ //<<<
	SERVOSHIELD_DDR |= _BV(MR_BANK_0) | _BV(MR_BANK_1) | _BV(CP0);
/* Configure the data direction for servo port (B).
 * Pin MR (PB0) and PB1 (CP0) are configured for output
 */
} //>>>


void servo_shield_init( void )
{ //<<<
	unsigned char sreg;

//   TCCR1A |= _BV(COM1A0);
//   TCCR1A |= _BV(COM1A0) | _BV(COM1A1);
// configure "set OC1A output high on compare match"
	TCCR1A &= ~_BV(COM1A0);
	TCCR1A &= ~_BV(COM1A1);
// confiure OC1A output pins for normal operation

	TCCR1B = _BV(CS11) | _BV(WGM12);
/* CS11 - use CLK/8 prescale value
 * WGM1[3-0] = [0,1,0,0] = CTC mode.
 * Generate interrupt on T1 = OCR1A, set output pin high
 * and clear the counter. Start counting from zero until
 * the OCR1A value is reached again.
 */

	SERVOSHIELD_CTRL |= _BV(MR_BANK_0);
	SERVOSHIELD_CTRL |= _BV(MR_BANK_1);
	_delay_us(10);
	SERVOSHIELD_CTRL &= ~_BV(MR_BANK_0);
//	SERVOSHIELD_CTRL &= ~_BV(MR_BANK_1);
// Send 10us strobe to 4017 - decade counter - to reset
	 
	sreg = SREG;
	cli();
	
	TCNT1 = (uint16_t) 0;
	// Set the Timer/counter1 to zero

	OCR1A = (uint16_t) NEUTRAL;
	/* Set the compare register counter to prescalled value. 
	 *
	 * The compae match limit is set dynamically in interrupt 
	 * handler. It generates the control signals for attached
	 * servos.
	 */

	SREG = sreg;
} //>>>


void servo_shield_cntrl( type_t mode )
{ //<<<
	switch( mode ) 
	{
		case DISABLE:
#if defined( __AVR_ATmega8__ )
			TIMSK &= ~_BV(OCIE1A);
#elif defined( __AVR_ATmega168__ )
			TIMSK1 &= ~_BV(OCIE1A);
#else
#error "Unsupported AVR device. This code compiles for ATmega8, ATmega168 or ATmega328"
#endif
			// Disable interrupt called by compare match.
		break;

		case ENABLE:
#if defined( __AVR_ATmega8__ )
			TIMSK |= _BV(OCIE1A);
#elif defined( __AVR_ATmega168__ )
			TIMSK1 |= _BV(OCIE1A);
#else
#error "Unsupported AVR device. This code compiles for ATmega8, ATmega168 or ATmega328"
#endif
			// Enable interrupt called by compare match.
		break;
	}
} //>>>


void fill_servo_page( move_t m )
{ //<<<
	uint8_t i;
	uint8_t servo_latch_ptr;

	servo_latch_ptr = 10;

	sreg = SREG;
	cli();
	// Disable interrupts while writing the latch buffer. 
	
	for( i = 0; i < COUNT_OF_ARMS; i++ ) {
		servo_latch_ptr += 1;
		servo_latch_ptr %= COUNT_OF_SERVOS;
		servo.past[servo_latch_ptr] = servo.future[servo_latch_ptr];
		servo.future[servo_latch_ptr] = (uint16_t) (tmp_step.arm[i].servo_shoulder > 0 ? tmp_step.arm[i].servo_shoulder : servo.past[servo_latch_ptr]) ;
		 
		servo_latch_ptr += 1;
		servo_latch_ptr %= COUNT_OF_SERVOS;

		servo.past[servo_latch_ptr] = servo.future[servo_latch_ptr];
		servo.future[servo_latch_ptr] = (uint16_t) (tmp_step.arm[i].servo_elbow > 0 ? tmp_step.arm[i].servo_elbow : servo.past[servo_latch_ptr]);

		servo_latch_ptr += 1;
		servo_latch_ptr %= COUNT_OF_SERVOS;

		servo.past[servo_latch_ptr] = servo.future[servo_latch_ptr];
		servo.future[servo_latch_ptr] = (uint16_t) (tmp_step.arm[i].servo_anckle > 0 ? tmp_step.arm[i].servo_anckle : servo.past[servo_latch_ptr]) ;
	}
	/* Fill the servo latch buffer. Apply rotation by 1 byte left, so that the slot No.12 contain
	 * the OCR1A data for first servo. The slot No.1 than contain value for second servo, etc.
	 */ 

	SREG = sreg;
} //>>>


void fill_servo_page_P( move_t m )
{ //<<<
	uint8_t i;
	uint8_t servo_latch_ptr;

	servo_latch_ptr = 10;

	sreg = SREG;
	cli();
	// Disable interrupts while writing the latch buffer. 
	
	for( i = 0; i < COUNT_OF_ARMS; i++ ) {
		servo_latch_ptr += 1;
		servo_latch_ptr %= COUNT_OF_SERVOS;
		servo.past[servo_latch_ptr] = servo.future[servo_latch_ptr];
//		servo.future[servo_latch_ptr] = (uint16_t) pgm_read_word( &run[m.movement].step[m.stepper_current].arm[i].servo_shoulder );
		servo.future[servo_latch_ptr] = (uint16_t) pgm_read_word( &c_step.arm[i].servo_shoulder );
		 
		servo_latch_ptr += 1;
		servo_latch_ptr %= COUNT_OF_SERVOS;

		servo.past[servo_latch_ptr] = servo.future[servo_latch_ptr];
//		servo.future[servo_latch_ptr] = (uint16_t) pgm_read_word( &run[m.movement].step[m.stepper_current].arm[i].servo_elbow );
		servo.future[servo_latch_ptr] = (uint16_t) pgm_read_word( &c_step.arm[i].servo_elbow );

		servo_latch_ptr += 1;
		servo_latch_ptr %= COUNT_OF_SERVOS;

		servo.past[servo_latch_ptr] = servo.future[servo_latch_ptr];
//		servo.future[servo_latch_ptr] = (uint16_t) pgm_read_word( &run[m.movement].step[m.stepper_current].arm[i].servo_anckle );
		servo.future[servo_latch_ptr] = (uint16_t) pgm_read_word( &c_step.arm[i].servo_anckle );
	}
	/* Fill the servo latch buffer. Apply rotation by 1 byte left, so that the slot No.12 contain
	 * the OCR1A data for first servo. The slot No.1 than contain value for second servo, etc.
	 */ 

	SREG = sreg;
} //>>>


void fill_servo_buffer( move_t m, speed_t s )
{ //<<<
	uint8_t i;
	int16_t temp;

	sreg = SREG;
	cli();
	// Disable interrupts while writing the latch buffer. 
	
	for( i = 0; i < COUNT_OF_SERVOS; i++ ) {
		temp = servo.future[i] - servo.past[i]; 
      temp *= s.index;
		temp /= s.max;
      temp += servo.past[i];
		servo.current[i] = (uint16_t) temp;
	}
	/* Fill the servo latch buffer based on the linear regression 
	 * beween two tabelated tabeleated entries. It already works
	 * with shifted buffer ( one byte left ).
	 */ 

	SREG = sreg; // enable interrupts again.
} //>>>

#ifdef FLOATINGPOINT_MATH

angles_t p2a( vector_t * v )
{ // <<<
	angles_t a;

	double hypot_j_z,hypot_x_y;
	double z;
	double temp;
	double a0;
	double gamma_shift;
	double signum;

	signum = 1.0;
	gamma_shift = 0;

	// Determine the quadrant and derive the shift in gama 
	if(( v->y < 0 ) && ( v->x > 0 )) {
		v->y = abs(v->y);
		gamma_shift = 2*M_PI;	
		signum = -1;
	}

	if(( v->y > 0 ) && ( v->x < 0 )) {
		v->x = abs(v->x);
		gamma_shift = M_PI;	
		signum = -1;
	}

	if(( v->y < 0 ) && ( v->x < 0 )) {
		v->x = abs(v->x);
		v->y = abs(v->y);
		gamma_shift = M_PI;	
	}

	hypot_x_y = sqrt((v->x*v->x) + (v->y*v->y)) - COXA;
	z = BASE - v->z;
	hypot_j_z = sqrt((hypot_x_y*hypot_x_y) + (z*z));

	temp = atan( v->y / v->x );
	if( gamma_shift > 0 ) {
		temp = gamma_shift + ( signum * temp );
	}
	a.gamma = servo_ctrl( temp );

	a0 = atan( hypot_x_y / z );

	temp = FEMUR_SQ;
	temp += hypot_x_y*hypot_x_y;
	temp += z*z;
	temp -= TIBIA_SQ;
	temp = temp / (2 * hypot_j_z * FEMUR);
	temp = acos(temp);
  	a.alpha = servo_ctrl( temp + a0 );	

	temp = SUM_TIBIA_SQ_FEMUR_SQ;
	temp -= z*z;
	temp -= hypot_x_y*hypot_x_y;
	temp = temp / TWO_FEMUR_TIBIA;
	temp = acos(temp);
	a.beta = servo_ctrl( temp );

	return a;
} // >>>


uint16_t servo_ctrl ( double angle )
{ // <<<
	double temp;

	// TowerPro 9g Servo data
	temp = 0.6364*angle + 0.4524;

//	temp = (double) F_CPU * temp / 8000;
	temp = (double) 14745600.0 * temp / 8000;
	return (uint16_t) (lrint( temp ) + 1);
} //>>>

#endif

#ifdef FIXEDPOINT_MATH

angles_t f16_p2a( f16_vector_t * v )
{ // <<<
	angles_t a;

	fix16_t hypot_j_z,hypot_x_y;
	fix16_t z;
	fix16_t temp;
	fix16_t a0;
	fix16_t gamma_shift;
	fix16_t signum;

	signum = fix16_one;
	gamma_shift = 0;

	// Determine the quadrant and derive the shift in gama 
	if(( (fix16_t) v->y < 0 ) && ( (fix16_t) v->x > 0 )) {
		v->y = abs(v->y);
		v->y = fix16_mul( (fix16_t) v->y, (fix16_t) fix16_minus_one ); // 2.33194509
		gamma_shift = F16_2_PI;	
		signum = fix16_minus_one;
	}

	if(( (fix16_t) v->y > 0 ) && ( (fix16_t) v->x < 0 )) {
		v->x = fix16_mul( (fix16_t) v->x, (fix16_t) fix16_minus_one ); // 2.33194509
		gamma_shift = F16_PI;	
		signum = fix16_minus_one;
	}

	if(( (fix16_t) v->y < 0 ) && ( (fix16_t) v->x < 0 )) {
		v->x = fix16_mul( (fix16_t) v->x, (fix16_t) fix16_minus_one ); // 2.33194509
		v->y = fix16_mul( (fix16_t) v->y, (fix16_t) fix16_minus_one ); // 2.33194509
		gamma_shift = F16_PI;	
	}

	hypot_x_y = fix16_sub(fix16_sqrt( fix16_add(fix16_mul(v->x,v->x), fix16_mul(v->y,v->y))), F16_COXA);
	z = fix16_sub(F16_BASE,v->z);
	hypot_j_z = fix16_sqrt( fix16_add(fix16_mul(hypot_x_y,hypot_x_y), fix16_mul(z,z)));

	temp = fix16_atan( fix16_div( v->y, v->x )); 
	if( gamma_shift > 0 ) {
		temp = fix16_add(gamma_shift, fix16_mul(  signum, temp ));
	}
	a.gamma = f16_servo_ctrl( temp, INVERT );

	a0 = fix16_atan( fix16_div( hypot_x_y, z ));

	temp = F16_FEMUR_SQ;					
	temp = fix16_add( fix16_mul( hypot_x_y, hypot_x_y ), temp );
	temp = fix16_add( fix16_mul( z,z ), temp );
	temp = fix16_sub( temp, F16_TIBIA_SQ );
	temp = fix16_div( temp, F16_TWO_FEMUR);
	temp = fix16_div( temp, hypot_j_z);
	temp = fix16_acos( temp );

	a.alpha = f16_servo_ctrl( fix16_add(temp,a0), INVERT );

	temp = F16_SUM_TIBIA_SQ_FEMUR_SQ;
	temp = fix16_sub( temp, fix16_mul(z,z));
	temp = fix16_sub( temp, fix16_mul(hypot_x_y,hypot_x_y));
	temp = fix16_div( temp, F16_TWO_FEMUR_TIBIA );
	temp = fix16_acos( temp );

	a.beta = f16_servo_ctrl( temp, NORMAL );

	return a;
} // >>>


uint16_t f16_servo_ctrl ( fix16_t angle, uint8_t invert )
{ // <<<
	fix16_t f16_temp;

	// TowerPro 9g Servo data
	if( INVERT == invert ) {
		angle = fix16_sub( F16_PI, angle );
	}
	f16_temp = fix16_add( fix16_mul( fix16_servo_a, angle ), fix16_servo_b );

	f16_temp = fix16_mul(fix16_clock_div_8k, f16_temp );
	return (uint16_t) (f16_temp >> 16);
} // >>>


fix16_t f16_sway( fix16_t step, uint8_t stepper_current )
{ // <<<
	fix16_t temp;
	fix16_t step_c;

	// x = (step/3) * (Ytable(1,k));
	/* TODO - perhaps just one table is required?
	if( move.cntrl == REVERSE ) {
		step_c = (int32_t) pgm_read_dword( &fix16_Ytable_rev[stepper_current] );
	} else {
		step_c = (int32_t) pgm_read_dword( &fix16_Ytable_fwd[stepper_current] );
	} */
	step_c = (int32_t) pgm_read_dword( &fix16_Ytable[stepper_current] );
	temp = fix16_mul(fix16_div( step, 0x00030000 ), step_c );	

	return temp;
} // >>>


fix16_t f16_step( fix16_t step, uint8_t arm, uint8_t k )
{ // <<<
	fix16_t temp;
	uint8_t idx;
	fix16_t step_c;

	idx = arm * XTABLE_X_SIZE + k;
//	sprintf_P( (char*) buffer, PSTR("%d"), idx );
	step_c = (int32_t) pgm_read_dword( &fix16_Xtable[idx] );

//	sprintf_P( (char*) buffer, PSTR("%d"), step_c );
//	step_c = fix16_Xtable[idx];

	temp = fix16_sub( fix16_div( step, 0x00020000 ), fix16_mul( step_c, step ));

	return temp;
} // >>>


f16_vector_t f16_rotate_m( f16_vector_t * v, fix16_t * m )
{ // <<<
	f16_vector_t temp;

	temp.x = fix16_add(fix16_mul(v->x,*(m)),fix16_mul(v->y,*(m+2)));
	temp.y = fix16_add(fix16_mul(v->x,*(m+1)),fix16_mul(v->y,*(m+3)));
	temp.z = 0x00000000;

	return temp;
} // >>>

#ifdef FIXMATH_FULL_LIBRARY
f16_vector_t f16_rotate_xy( f16_vector_t * v, fix16_t angle )
{ //<<<
	f16_vector_t temp;

	fix16_t rot[4];

	rot[0] = fix16_cos( angle );
	rot[1] = fix16_sin( angle );
	rot[2] = fix16_mul( 0xFFFF0000, rot[1] );
	rot[3] = rot[0];

	temp.x = fix16_add(fix16_mul(v->x,rot[0]),fix16_mul(v->y,rot[2]));
	temp.y = fix16_add(fix16_mul(v->x,rot[1]),fix16_mul(v->y,rot[3]));
	temp.z = 0x00000000;

	return temp;
} // >>>


f16_vector_t f16_rotate_y( f16_vector_t * v, fix16_t angle )
{ // <<<
	f16_vector_t temp;

	fix16_t rot[2];

	rot[0] = fix16_mul( 0xFFFF0000, fix16_sin( angle ) );
	rot[1] = fix16_cos( angle );

	temp.x = fix16_mul(v->y,rot[0]);
	temp.y = fix16_mul(v->y,rot[1]);
	temp.z = 0x00000000;

	return temp;
} // >>>

#endif
#endif

